//
// Created by deng on 2022/12/25.
//

//
// Created by deng on 2022/9/22.
//
#include "mpu6500.h"
#include "bsp_i2c.h"
#include "usart.h"

ImuData_t MpuData;

//初始化MPU9250
//返回值:0,成功
//    其他,错误代码
uint8_t MPU9250_Init(void)
{
    uint8_t res=0;
    i2c_GPIO_Config();     //初始化IIC总线
    MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X80);//复位MPU9250
    HAL_Delay(100);  //延时100ms
    MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X00);//唤醒MPU9250
    MPU_Set_Gyro_Fsr(3);					        	//陀螺仪传感器,±2000dps
    MPU_Set_Accel_Fsr(0);					       	 	//加速度传感器,±2g
    MPU_Set_Rate(50);						       	 	//设置采样率50Hz
    MPU_Write_Byte(MPU9250_ADDR,MPU_INT_EN_REG,0X00);   //关闭所有中断
    MPU_Write_Byte(MPU9250_ADDR,MPU_USER_CTRL_REG,0X00);//I2C主模式关闭
    MPU_Write_Byte(MPU9250_ADDR,MPU_FIFO_EN_REG,0X00);	//关闭FIFO
    MPU_Write_Byte(MPU9250_ADDR,MPU_INTBP_CFG_REG,0X82);//INT引脚低电平有效，开启bypass模式，可以直接读取磁力计
    res=MPU_Read_Byte(MPU9250_ADDR,MPU_DEVICE_ID_REG);  //读取MPU6500的ID

   // printf("MPU9250_ID:0X%x\r\n",res);

    if(res==MPU6500_ID1 || res==MPU6500_ID2 || res==MPU6500_ID3) //器件ID正确
    {
        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT1_REG,0X01);  	//设置CLKSEL,PLL X轴为参考
        MPU_Write_Byte(MPU9250_ADDR,MPU_PWR_MGMT2_REG,0X00);  	//加速度与陀螺仪都工作
        MPU_Set_Rate(50);						       	//设置采样率为50Hz
    }else return 1;

    res=MPU_Read_Byte(AK8963_ADDR,MAG_WIA);    			//读取AK8963 ID

    //printf("AK8963_ID:0X%x\r\n",res);

    if(res==AK8963_ID)
    {
        MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11);		//设置AK8963为单次测量模式
    }else return 1;

    return 0;
}

//设置MPU9250陀螺仪传感器满量程范围
//fsr:0,±250dps;1,±500dps;2,±1000dps;3,±2000dps
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_Gyro_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU9250_ADDR,MPU_GYRO_CFG_REG,fsr<<3);//设置陀螺仪满量程范围
}
//设置MPU9250加速度传感器满量程范围
//fsr:0,±2g;1,±4g;2,±8g;3,±16g
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_Accel_Fsr(uint8_t fsr)
{
    return MPU_Write_Byte(MPU9250_ADDR,MPU_ACCEL_CFG_REG,fsr<<3);//设置加速度传感器满量程范围
}

//设置MPU9250的数字低通滤波器
//lpf:数字低通滤波频率(Hz)
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_LPF(uint16_t lpf)
{
    uint8_t data=0;
    if(lpf>=188)data=1;
    else if(lpf>=98)data=2;
    else if(lpf>=42)data=3;
    else if(lpf>=20)data=4;
    else if(lpf>=10)data=5;
    else data=6;
    return MPU_Write_Byte(MPU9250_ADDR,MPU_CFG_REG,data);//设置数字低通滤波器
}

//设置MPU9250的采样率(假定Fs=1KHz)
//rate:4~1000(Hz)
//返回值:0,设置成功
//    其他,设置失败
uint8_t MPU_Set_Rate(uint16_t rate)
{
    uint8_t data;
    if(rate>1000)rate=1000;
    if(rate<4)rate=4;
    data=1000/rate-1;
    data=MPU_Write_Byte(MPU9250_ADDR,MPU_SAMPLE_RATE_REG,data);	//设置数字低通滤波器
    return MPU_Set_LPF(rate/2);	//自动设置LPF为采样率的一半
}

//得到温度值
//返回值:温度值(扩大了100倍)
short MPU_Get_Temperature(void)
{
    uint8_t buf[2];
    short raw;
    float temp;
    MPU_Read_Len(MPU9250_ADDR,MPU_TEMP_OUTH_REG,2,buf);
    raw=((uint16_t)buf[0]<<8)|buf[1];
    temp=21+((double)raw)/333.87;
    return temp*100;;
}
//得到陀螺仪值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    uint8_t buf[6],res;
    res=MPU_Read_Len(MPU9250_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
    if(res==0)
    {
        *gx=((uint16_t)buf[0]<<8)|buf[1];
        *gy=((uint16_t)buf[2]<<8)|buf[3];
        *gz=((uint16_t)buf[4]<<8)|buf[5];
    }
    return res;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    uint8_t buf[6],res;
    res=MPU_Read_Len(MPU9250_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
    if(res==0)
    {
        *ax=((uint16_t)buf[0]<<8)|buf[1];
        *ay=((uint16_t)buf[2]<<8)|buf[3];
        *az=((uint16_t)buf[4]<<8)|buf[5];
    }
    return res;;
}

void AccUpdata(){
    short int ax,ay,az;
    MPU_Get_Accelerometer(&ax,&ay,&az);
    MpuData.ax = (float)ax/(32768/2);
    MpuData.ay = (float)ay/(32768/2);
    MpuData.az = (float)az/(32768/2);
}

void AccUpdataAngle(){
    AccUpdata();
    MpuData.axAngle = -90 + ((MpuData.ax+1)/(1-(-1)))*180;
    MpuData.ayAngle = -90 + ((MpuData.ay+1)/(1-(-1)))*180;
    MpuData.azAngle = -90 + ((MpuData.az+1)/(1-(-1)))*180;

    AccUpdataAngleMean();


}

void AccUpdataAngleMean(){
    static uint16_t AccUpdataTimes;


    static float axBuff[ACC_DATA_MEAN_BUFF_SIZE];
    static float ayBuff[ACC_DATA_MEAN_BUFF_SIZE];
    static float azBuff[ACC_DATA_MEAN_BUFF_SIZE];
    static uint16_t idx=0;
    uint16_t CalIdx;
    double doubleBuff;
    uint16_t TakeOutSumHalf = (ACC_DATA_MEAN_BUFF_SIZE -ACC_DATA_MEAN_SIZE)/2;


    axBuff[idx] = MpuData.axAngle;
    ayBuff[idx] = MpuData.ayAngle;
    azBuff[idx] = MpuData.azAngle;

    if(++AccUpdataTimes >= ACC_DATA_MEAN_BUFF_SIZE) {
        AccUpdataTimes = 0;

        select_sort(axBuff, ACC_DATA_MEAN_BUFF_SIZE, TakeOutSumHalf, SMALL_TO_LARGE, LEFT_TO_RIGHT, NULL, 0);
        select_sort(axBuff, ACC_DATA_MEAN_BUFF_SIZE, TakeOutSumHalf, LARGE_TO_SMALL, RIGHT_TO_LEFT, NULL, 0);
        for (CalIdx = TakeOutSumHalf, doubleBuff = 0; CalIdx < ACC_DATA_MEAN_BUFF_SIZE - TakeOutSumHalf; CalIdx++) {
            doubleBuff += axBuff[CalIdx];
        }
        MpuData.axAngleMean = doubleBuff / (ACC_DATA_MEAN_BUFF_SIZE - TakeOutSumHalf * 2);

        select_sort(ayBuff, ACC_DATA_MEAN_BUFF_SIZE, TakeOutSumHalf, SMALL_TO_LARGE, LEFT_TO_RIGHT, NULL, 0);
        select_sort(ayBuff, ACC_DATA_MEAN_BUFF_SIZE, TakeOutSumHalf, LARGE_TO_SMALL, RIGHT_TO_LEFT, NULL, 0);
        for (CalIdx = TakeOutSumHalf, doubleBuff = 0; CalIdx < ACC_DATA_MEAN_BUFF_SIZE - TakeOutSumHalf; CalIdx++) {
            doubleBuff += ayBuff[CalIdx];
        }
        MpuData.ayAngleMean = doubleBuff / (ACC_DATA_MEAN_BUFF_SIZE - TakeOutSumHalf * 2);

        select_sort(azBuff, ACC_DATA_MEAN_BUFF_SIZE, TakeOutSumHalf, SMALL_TO_LARGE, LEFT_TO_RIGHT, NULL, 0);
        select_sort(azBuff, ACC_DATA_MEAN_BUFF_SIZE, TakeOutSumHalf, LARGE_TO_SMALL, RIGHT_TO_LEFT, NULL, 0);
        for (CalIdx = TakeOutSumHalf, doubleBuff = 0; CalIdx < ACC_DATA_MEAN_BUFF_SIZE - TakeOutSumHalf; CalIdx++) {
            doubleBuff += azBuff[CalIdx];
        }
        MpuData.azAngleMean = doubleBuff / (ACC_DATA_MEAN_BUFF_SIZE - TakeOutSumHalf * 2);

        //printf("%.3f(%.3f) %.3f(%.3f) %.3f(%.3f) \n",MpuData.ax,MpuData.axAngle,MpuData.ay,MpuData.ayAngle,MpuData.az,MpuData.azAngle);
        //printf("%.3f %.3f %.3f \n",MpuData.axAngleMean,MpuData.ayAngleMean,MpuData.azAngleMean);

    }

    if(++idx >= ACC_DATA_MEAN_BUFF_SIZE)
        idx = 0;

}

//得到磁力计值(原始值)
//mx,my,mz:磁力计x,y,z轴的原始读数(带符号)
//返回值:0,成功
//    其他,错误代码
uint8_t MPU_Get_Magnetometer(short *mx,short *my,short *mz)
{
    uint8_t buf[6],res;
    res=MPU_Read_Len(AK8963_ADDR,MAG_XOUT_L,6,buf);
    if(res==0)
    {
        *mx=((uint16_t)buf[1]<<8)|buf[0];
        *my=((uint16_t)buf[3]<<8)|buf[2];
        *mz=((uint16_t)buf[5]<<8)|buf[4];
    }
    MPU_Write_Byte(AK8963_ADDR,MAG_CNTL1,0X11); //AK8963每次读完以后都需要重新设置为单次测量模式
    return res;;
}

//IIC连续写
//addr:器件地址
//reg:寄存器地址
//len:写入长度
//buf:数据区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Write_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
    uint8_t i;
    i2c_Start();
    i2c_SendByte((addr<<1)|0); //发送器件地址+写命令
    if(i2c_WaitAck())          //等待应答
    {
        i2c_Stop();
        return 1;
    }
    i2c_SendByte(reg);         //写寄存器地址
    i2c_WaitAck();             //等待应答
    for(i=0;i<len;i++)
    {
        i2c_SendByte(buf[i]);  //发送数据
        if(i2c_WaitAck())      //等待ACK
        {
            i2c_Stop();
            return 1;
        }
    }
    i2c_Stop();
    return 0;
}

//IIC连续读
//addr:器件地址
//reg:要读取的寄存器地址
//len:要读取的长度
//buf:读取到的数据存储区
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Read_Len(uint8_t addr,uint8_t reg,uint8_t len,uint8_t *buf)
{
    i2c_Start();
    i2c_SendByte((addr<<1)|0); //发送器件地址+写命令
    if(i2c_WaitAck())          //等待应答
    {
        i2c_Stop();
        return 1;
    }
    i2c_SendByte(reg);         //写寄存器地址
    i2c_WaitAck();             //等待应答
    i2c_Start();
    i2c_SendByte((addr<<1)|1); //发送器件地址+读命令
    i2c_WaitAck();             //等待应答
    while(len)
    {
        if(len==1)*buf=i2c_ReadByte(0);//读数据,发送nACK
        else *buf=i2c_ReadByte(1);		//读数据,发送ACK
        len--;
        buf++;
    }
    i2c_Stop();                 //产生一个停止条件
    return 0;
}

//IIC写一个字节
//devaddr:器件IIC地址
//reg:寄存器地址
//data:数据
//返回值:0,正常
//    其他,错误代码
uint8_t MPU_Write_Byte(uint8_t addr,uint8_t reg,uint8_t data)
{
    i2c_Start();
    i2c_SendByte((addr<<1)|0); //发送器件地址+写命令
    if(i2c_WaitAck())          //等待应答
    {
        i2c_Stop();
        return 1;
    }
    i2c_SendByte(reg);         //写寄存器地址
    i2c_WaitAck();             //等待应答
    i2c_SendByte(data);        //发送数据
    if(i2c_WaitAck())          //等待ACK
    {
        i2c_Stop();
        return 1;
    }
    i2c_Stop();
    return 0;
}

//IIC读一个字节
//reg:寄存器地址
//返回值:读到的数据
uint8_t MPU_Read_Byte(uint8_t addr,uint8_t reg)
{
    uint8_t res;
    i2c_Start();
    i2c_SendByte((addr<<1)|0); //发送器件地址+写命令
    i2c_WaitAck();             //等待应答
    i2c_SendByte(reg);         //写寄存器地址
    i2c_WaitAck();             //等待应答
    i2c_Start();
    i2c_SendByte((addr<<1)|1); //发送器件地址+读命令
    i2c_WaitAck();             //等待应答
    res=i2c_ReadByte(0);		//读数据,发送nACK
    i2c_Stop();                 //产生一个停止条件
    return res;
}


void select_sort(float arr[],uint16_t len,uint16_t sortSum,SortType LargeSmallDir,SortType LeftRightDir,uint8_t *SyncArr,uint16_t SyncArrLen)
{
    int i,j,selctIdx;
    double temp;
    uint16_t havesortSum;

    if(LargeSmallDir == SMALL_TO_LARGE && LeftRightDir == LEFT_TO_RIGHT)	//从左到右，从小到大
    {
        for(i=0;i<sortSum;i++)                //注意是i < len-1
        {
            selctIdx=i;                    //先默认第i个为剩下中最小的
            for(j=i+1;j<len;j++)            //注意是i < len
            {
                if(arr[j]<arr[selctIdx])
                    selctIdx=j;
            }
            if(selctIdx!=i)				//如果最小的数不是当前的arr[i]，就交换，这样效率高很多
            {
                temp=arr[i];
                arr[i]=arr[selctIdx];
                arr[selctIdx]=temp;

                if(SyncArr != NULL && i < SyncArrLen )
                {
                    temp=SyncArr[i];
                    SyncArr[i]=SyncArr[selctIdx];
                    SyncArr[selctIdx]=temp;
                }
            }
            //SEGGER_RTT_printf(0,"arr[%d] = %d \n",i,(int)arr[i]);

        }
    }

    else if(LargeSmallDir == LARGE_TO_SMALL && LeftRightDir == RIGHT_TO_LEFT)	//从右到左，从大到小
    {
        for(i=len-1;i > len - sortSum - 1;i--)                //注意是i > 0
        {
            selctIdx=i;                    //先默认第i个为剩下中最大的
            for(j = i-1;j >= 0;j--)            //注意是i > 1
            {
                if(arr[j] > arr[selctIdx])
                    selctIdx=j;
            }
            if(selctIdx!=i)				//如果最小的数不是当前的arr[i]，就交换，这样效率高很多
            {
                temp=arr[i];
                arr[i]=arr[selctIdx];
                arr[selctIdx]=temp;

                if(SyncArr != NULL && len - i <= SyncArrLen )
                {
                    temp=SyncArr[i];
                    SyncArr[i]=SyncArr[selctIdx];
                    SyncArr[selctIdx]=temp;
                }
            }

            //SEGGER_RTT_printf(0,"arr[%d] = %d \n",i,(int)arr[i]);

        }
    }
    else if(LargeSmallDir == SMALL_TO_LARGE && LeftRightDir == RIGHT_TO_LEFT)	//从右到左，从小到大
    {
        for(i=len-1;i > len - sortSum - 1;i--)                //注意是i > 0
        {
            selctIdx=i;                    //先默认第i个为剩下中最小的
            for(j = i-1;j >= 0;j--)            //注意是i > 1
            {
                if(arr[j] < arr[selctIdx])
                    selctIdx=j;
            }
            if(selctIdx!=i)				//如果最小的数不是当前的arr[i]，就交换，这样效率高很多
            {
                temp=arr[i];
                arr[i]=arr[selctIdx];
                arr[selctIdx]=temp;

                if(SyncArr != NULL && len - i <= SyncArrLen )
                {
                    temp=SyncArr[i];
                    SyncArr[i]=SyncArr[selctIdx];
                    SyncArr[selctIdx]=temp;
                }
            }
            //SEGGER_RTT_printf(0,"arr[%d] = %d \n",i,(int)arr[i]);

        }
    }
    else if(LargeSmallDir == LARGE_TO_SMALL && LeftRightDir == LEFT_TO_RIGHT)
    {
        for(i=0;i<sortSum;i++)                //注意是i < len-1
        {
            selctIdx=i;                    //先默认第i个为剩下中最大的
            for(j=i+1;j<len;j++)            //注意是i < len
            {
                if(arr[j] > arr[selctIdx])
                    selctIdx=j;
            }
            if(selctIdx!=i)				//如果最小的数不是当前的arr[i]，就交换，这样效率高很多
            {
                temp=arr[i];
                arr[i]=arr[selctIdx];
                arr[selctIdx]=temp;

                if(SyncArr != NULL && i < SyncArrLen )
                {
                    temp=SyncArr[i];
                    SyncArr[i]=SyncArr[selctIdx];
                    SyncArr[selctIdx]=temp;
                }

            }
            //SEGGER_RTT_printf(0,"arr[%d] = %d \n",i,(int)arr[i]);

        }
    }


}